#ifndef MOTORS_H_
#define MOTORS_H_

#include <config.h>
#include <motor_config.h>
#include "vertify.h"

void setMotorTimeout(int, int);

int motor_init(){
  // For Motor Controllers
  Serial1.begin(MOTOR_CONTROLLER_BAUD);
  
  delay(100);
  setMotorTimeout(MOTOR_CONTROLLER_RIGHT_ADDRESS, MOTOR_TIMEOUT);
  setMotorTimeout(MOTOR_CONTROLLER_LEFT_ADDRESS, MOTOR_TIMEOUT);
}

void setMotorTimeout(int address, int timeout){
  int commandNum = MOTOR_TIMEOUT_SELECT;
  int checkSum = ((address + commandNum + timeout)  &  0b01111111);

  Serial1.write(address);
  Serial1.write(commandNum);
  Serial1.write(timeout);
  Serial1.write(checkSum);
 }

int commandFR(int val){
  int commandNum;
  int commandVal = val;
  int address = MOTOR_CONTROLLER_RIGHT_ADDRESS;
  
  if(commandVal > 128){
    // Forward
    commandNum = MOTOR_FR_FORWARD;
    commandVal -= 128;
  }else{
    // BackWards
    commandNum = MOTOR_FR_BACKWARD;
    commandVal = 128 - commandVal;
  }

  // Prevent Power Saving
  commandVal = constrain(commandVal, 1, 255);

  int checkSum = ((address + commandNum + commandVal) &  0b01111111);

  // Serial1
#ifdef DEBUG
  Serial.println("FR motor:");
  Serial.println(commandVal);
  Serial.println(commandNum);
#endif
  Serial1.write(address);
  Serial1.write(commandNum);
  Serial1.write(commandVal);
  Serial1.write(checkSum);
  return 0;
}

int commandFL(int val){
  int commandNum;
  int commandVal = val;
  int address = MOTOR_CONTROLLER_LEFT_ADDRESS;

  if(commandVal > 128){
    // Forward
    commandNum = MOTOR_FL_FORWARD;
    commandVal -= 128;
  }else{
    // BackWards
    commandNum = MOTOR_FL_BACKWARD;
    commandVal = 128 - commandVal;
  }

  // Prevent Power Saving
  commandVal = constrain(commandVal, 1, 255);

  int checkSum = ((address + commandNum + commandVal) &  0b01111111);

  // Serial1
#ifdef DEBUG
  Serial.println("FL motor:");
  Serial.println(commandVal);
  Serial.println(commandNum);
#endif
  Serial1.write(address);
  Serial1.write(commandNum);
  Serial1.write(commandVal);
  Serial1.write(checkSum);
  return 0;
}

int commandRR(int val){
 int commandNum;
  int commandVal = val;
  int address = MOTOR_CONTROLLER_RIGHT_ADDRESS;

  if(commandVal > 128){
    // Forward
    commandNum = MOTOR_RR_FORWARD;
    commandVal -= 128;
  }else{
    // BackWards
    commandNum = MOTOR_RR_BACKWARD;
    commandVal = 128 - commandVal;
  }

  // Prevent Power Saving
  commandVal = constrain(commandVal, 1, 255);

  int checkSum = ((address + commandNum + commandVal) &  0b01111111);

  // Serial1
#ifdef DEBUG
  Serial.println("RR motor:");
  Serial.println(commandVal);
  Serial.println(commandNum);
#endif
  Serial1.write(address);
  Serial1.write(commandNum);
  Serial1.write(commandVal);
  Serial1.write(checkSum);
  return 0;
}

int commandRL(int val){
  int commandNum;
  int commandVal = val;
  int address = MOTOR_CONTROLLER_LEFT_ADDRESS;

  if(commandVal > 128){
    // Forward
    commandNum = MOTOR_RL_FORWARD;
    commandVal -= 128;
  }else{
    // BackWards
    commandNum = MOTOR_RL_BACKWARD;
    commandVal = 128 - commandVal;
  }

  // Prevent Power Saving
  commandVal = constrain(commandVal, 1, 255);

  int checkSum = ((address + commandNum + commandVal) &  0b01111111);

  // Serial1
#ifdef DEBUG
  Serial.println("RL motor:");
  Serial.println(commandVal);
  Serial.println(commandNum);
#endif
  Serial1.write(address);
  Serial1.write(commandNum);
  Serial1.write(commandVal);
  Serial1.write(checkSum);
  return 0;
}


int commandMotors(char* comps){
  int fr = (int)FR_D_MOTOR;
  int fl = (int)FL_D_MOTOR;
  int rr = (int)RR_D_MOTOR;
  int rl = (int)RL_D_MOTOR;

  int fr_val = getValue(comps, fr);
  int fl_val = getValue(comps, fl);
  int rr_val = getValue(comps, rr);
  int rl_val = getValue(comps, rl);

  int numErrors = 0;

  if(isValid(fr_val)){
    if(commandFR(fr_val) < 0){
      Serial.println("Unable to send value to fr_motor.");
      numErrors++;
    }
  }else{
    Serial.println("Invalid FR motor value.");
    numErrors++;
  }

  if(isValid(fl_val)){
    if(commandFL(fl_val) < 0){
      Serial.println("Unable to send value to fl_motor.");
      numErrors++;
    }
  }else{
    Serial.println("Invalid FL motor value.");
    numErrors++;
  }

  if(isValid(rr_val)){
    if(commandRR(rr_val) < 0){
      Serial.println("Unable to send value to rr_motor.");
      numErrors++;
    }
  }else{
    Serial.println("Invalid RR motor value.");
    numErrors++;
  }

  if(isValid(rl_val)){
    if(commandRL(rl_val) < 0){
      Serial.println("Unanble to send value to rl_motor.");
      numErrors++;
    }
  }else{
    Serial.println("Invalid RL motor value.");
    numErrors++;
  }

  if(numErrors > 0){
    Serial.println("Errors occurred in motor commanding. Num: " + numErrors);
    return -1;
  }

  return 0;
}
#endif
